package novemberBot;

import novemberBot.communication.RS485Responder;
import novemberBot.utils.ByteArray;
import lejos.nxt.TachoMotorPort;

/**represents the claw and arm to which it is attached*/
public class Arm implements RS485Responder{
	/**ID for RS485 connection manager*/
	private final byte ARM_ID=1;
	/**ratio between height of claw and degrees the attached motor must rotate*/
	public static final double HEIGHT_TO_DEG=220;
	/**motor used for opening and closing the claw*/
	private EnhancedMotor clawMotor;
	/**motor used for opening and closing the claw*/
	private EnhancedMotor liftMotor;
	/**current claw Height*/
	public double clawHeight=0;
	/**Default lift speed*/
	public static final double LIFT_SPEED=200;
	/**Default open speed*/
	public static final double OPEN_SPEED=150;
	/**info on current claw position*/
//	private double clawHeight=0, clawAngle=0;
	
	/**Lock for synchronized statement*/
	//private Object clHLock = new Object(), clALock = new Object();
	
	
	/**Arm constructor*/
	public Arm(	TachoMotorPort clawPort, double clawRatio,
				TachoMotorPort liftPort, double liftRatio){
		
		clawMotor=new EnhancedMotor(clawPort, clawRatio);
		liftMotor=new EnhancedMotor(liftPort, liftRatio);
		//clawMotor.setAutoStop(false);
		
		
	}
	
	/**opens or closes the claw uses default open speed
	 * @param angle - absolute angle in deg the sides of the claw should rotate to*/
	public void openClawTo(double angle){
		openClawTo(angle, OPEN_SPEED);
	}
	
	/**opens or closes the claw 
	 * @param angle - absolute angle in deg the sides of the claw should rotate to
	 * @param speed - speed in deg/s of the variation of the angle parameter*/
	public void openClawTo(double angle, double speed){
		//Debugger.breakPoint("open ang:"+angle+" speed:"+speed);
		clawMotor.stop();
		clawMotor.setGearSpeed(speed);
		clawMotor.rotateGearTo(angle, true, true);
		
	}
	
	
	/**used to lift and lower the claw uses default speed
	 * @param height - absolute height in cm the claw should go to*/
	public void liftClawTo(double height){
		liftClawTo(height, LIFT_SPEED);
	}
	
	/**used to lift and lower the claw
	 * @param height - absolute height in cm the claw should go to
	 * @param speed - speed in cm/s of the variation of the height parameter*/
	public void liftClawTo(double height, double speed){
		//Debugger.breakPoint("lift height:"+height+" speed:"+speed);
		liftMotor.stop();
		liftMotor.setGearSpeed(speed);
		liftMotor.rotateGear((clawHeight-height)*HEIGHT_TO_DEG, true);
		clawHeight=height;
	}
	
	
	
	//accessors
	
	/**Synchronized accessor for clawAngle*/
	public double getClawAngle(){
			return clawMotor.getGearTachoCount();
	}
	
	
	/**Revives data from the RS485 Connection and performs corresponding arm motions
	 * @param data the coded commands
	 * @see ArmRemote*/
	@Override
	public void newRS485Data(byte[] data) {
		//Debugger.breakPoint("Arm d rec d[0]"+ data[0]+" length:"+data.length);
		if(data[0]==0){
			if(data.length==9){
				openClawTo(ByteArray.toInt(ByteArray.subSection(1, 4, data))/100.0, 
					ByteArray.toInt(ByteArray.subSection(5, 8, data))/100.0);
			}else if(data.length==5){
				openClawTo(ByteArray.toInt(ByteArray.subSection(1, 4, data))/100.0);
			}
		}else if(data[0]==1){
			if(data.length ==9){
				liftClawTo(ByteArray.toInt(ByteArray.subSection(1, 4, data))/100.0, 
					ByteArray.toInt(ByteArray.subSection(5, 8, data))/100.0);
			}else if(data.length==5){
				liftClawTo(ByteArray.toInt(ByteArray.subSection(1, 4, data))/100.0);
			}
		}
		
	}

	@Override
	public byte getRSId() {
		return ARM_ID;
	}
}
